
#ifndef ODOMETRY_H_
#define ODOMETRY_H_

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <functional>
#include "chassis_controller/pivots.h"


namespace chassis_controller{

  class Odometry
  {
  public:
    explicit Odometry(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node)
        : node_(node)
    {}

    virtual ~Odometry(){};
    
    virtual void init(const rclcpp::Time &time, std::vector<Spivot_value> &pivot_value) = 0;

    virtual void update(std::vector<Spivot_value> &pivot_value,  std::vector<Spivot_param> &pivot_param , const rclcpp::Time &time , bool standstill,double odom_factor) = 0;

    virtual double getHeading() = 0;

    virtual double getX() = 0;

    virtual double getY() = 0;

    virtual double getVx() = 0;

    virtual double getVy() = 0;

    virtual double getVa() = 0;

    static Odometry* create(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, int chassis_type);

    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;

    inline auto& get_clock() { return *(node_->get_clock()); }
  };
}
#endif /* ODOMETRY_H_ */
